﻿using SoftFloat;
using System;
using System.Diagnostics;
using System.Diagnostics.CodeAnalysis;
using System.Globalization;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using System.Text;

namespace BepuUtilities
{
    public static class Vector
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> Abs<T>(Vector<T> value)
        {
            if (typeof(T) == typeof(byte) || typeof(T) == typeof(ushort) || typeof(T) == typeof(uint) || typeof(T) == typeof(ulong) || typeof(T) == typeof(UIntPtr))
            {
                return value;
            }
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                T value2 = Scalar<T>.Abs(value.GetElementUnsafe(i));
                result.SetElementUnsafe(i, value2);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> BitwiseAnd<T>(Vector<T> left, Vector<T> right)
        {
            return left & right;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<sfloat> ConditionalSelect(Vector<int> condition, Vector<sfloat> left, Vector<sfloat> right)
        {
            return ConditionalSelect(condition.As<int, sfloat>(), left, right);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> ConditionalSelect<T>(Vector<T> condition, Vector<T> left, Vector<T> right)
        {
            return (left & condition) | (right & ~condition);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<sfloat> Floor(Vector<sfloat> value)
        {
            Unsafe.SkipInit(out Vector<sfloat> result);
            for (int i = 0; i < Vector<sfloat>.Count; i++)
            {
                sfloat value2 = Scalar<sfloat>.Floor(value.GetElementUnsafe(i));
                result.SetElementUnsafe(i, value2);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<int> GreaterThan(Vector<int> left, Vector<int> right)
        {
            return GreaterThan<int>(left, right);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<int> GreaterThan(Vector<sfloat> left, Vector<sfloat> right)
        {
            return GreaterThan<sfloat>(left, right).As<sfloat, int>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> GreaterThan<T>(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                T value = Scalar<T>.GreaterThan(left.GetElementUnsafe(i), right.GetElementUnsafe(i)) ? Scalar<T>.AllBitsSet : default;
                result.SetElementUnsafe(i, value);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> LessThan<T>(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                T value = Scalar<T>.LessThan(left.GetElementUnsafe(i), right.GetElementUnsafe(i)) ? Scalar<T>.AllBitsSet : default;
                result.SetElementUnsafe(i, value);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<int> LessThan(Vector<sfloat> left, Vector<sfloat> right)
        {
            return LessThan<sfloat>(left, right).As<sfloat, int>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> Max<T>(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                T value = Scalar<T>.GreaterThan(left.GetElementUnsafe(i), right.GetElementUnsafe(i)) ? left.GetElementUnsafe(i) : right.GetElementUnsafe(i);
                result.SetElementUnsafe(i, value);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> Min<T>(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                T value = Scalar<T>.LessThan(left.GetElementUnsafe(i), right.GetElementUnsafe(i)) ? left.GetElementUnsafe(i) : right.GetElementUnsafe(i);
                result.SetElementUnsafe(i, value);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> SquareRoot<T>(Vector<T> value)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                T value2 = Scalar<T>.Sqrt(value.GetElementUnsafe(i));
                result.SetElementUnsafe(i, value2);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Widen(Vector<byte> source, out Vector<ushort> low, out Vector<ushort> high)
        {
            low = WidenLower(source);
            high = WidenUpper(source);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<ushort> WidenLower(Vector<byte> source)
        {
            Unsafe.SkipInit(out Vector<ushort> result);
            for (int i = 0; i < Vector<ushort>.Count; i++)
            {
                ushort elementUnsafe = source.GetElementUnsafe(i);
                result.SetElementUnsafe(i, elementUnsafe);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<ushort> WidenUpper(Vector<byte> source)
        {
            Unsafe.SkipInit(out Vector<ushort> result);
            for (int i = Vector<ushort>.Count; i < Vector<byte>.Count; i++)
            {
                ushort elementUnsafe = source.GetElementUnsafe(i);
                result.SetElementUnsafe(i - Vector<ushort>.Count, elementUnsafe);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static T Sum<T>(Vector<T> value)
        {
            T t = default;
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                t = Scalar<T>.Add(t, value.GetElementUnsafe(i));
            }
            return t;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> BitwiseOr<T>(Vector<T> left, Vector<T> right)
        {
            return left | right;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> OnesComplement<T>(Vector<T> value)
        {
            return ~value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<int> LessThanOrEqual(Vector<sfloat> left, Vector<sfloat> right)
        {
            return LessThanOrEqual<sfloat>(left, right).As<sfloat, int>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> LessThanOrEqual<T>(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                T value = Scalar<T>.LessThanOrEqual(left.GetElementUnsafe(i), right.GetElementUnsafe(i)) ? Scalar<T>.AllBitsSet : default;
                result.SetElementUnsafe(i, value);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<int> LessThanOrEqual(Vector<int> left, Vector<int> right)
        {
            return LessThanOrEqual<int>(left, right);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<int> Equals(Vector<int> left, Vector<int> right)
        {
            return Equals<int>(left, right);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> Equals<T>(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                T value = Scalar<T>.Equals(left.GetElementUnsafe(i), right.GetElementUnsafe(i)) ? Scalar<T>.AllBitsSet : default;
                result.SetElementUnsafe(i, value);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> AndNot<T>(Vector<T> left, Vector<T> right)
        {
            return left & ~right;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool LessThanAny<T>(Vector<T> left, Vector<T> right)
        {
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                if (Scalar<T>.LessThan(left.GetElementUnsafe(i), right.GetElementUnsafe(i)))
                {
                    return true;
                }
            }
            return false;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool LessThanAll<T>(Vector<T> left, Vector<T> right)
        {
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                if (!Scalar<T>.LessThan(left.GetElementUnsafe(i), right.GetElementUnsafe(i)))
                {
                    return false;
                }
            }
            return true;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool EqualsAny<T>(Vector<T> left, Vector<T> right)
        {
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                if (Scalar<T>.Equals(left.GetElementUnsafe(i), right.GetElementUnsafe(i)))
                {
                    return true;
                }
            }
            return false;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<int> Equals(Vector<sfloat> left, Vector<sfloat> right)
        {
            return Equals<sfloat>(left, right).As<sfloat, int>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<int> GreaterThanOrEqual(Vector<int> left, Vector<int> right)
        {
            return GreaterThanOrEqual<int>(left, right);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> GreaterThanOrEqual<T>(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                T value = Scalar<T>.GreaterThanOrEqual(left.GetElementUnsafe(i), right.GetElementUnsafe(i)) ? Scalar<T>.AllBitsSet : default;
                result.SetElementUnsafe(i, value);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<int> GreaterThanOrEqual(Vector<sfloat> left, Vector<sfloat> right)
        {
            return GreaterThanOrEqual<sfloat>(left, right).As<sfloat, int>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static T Dot<T>(Vector<T> left, Vector<T> right)
        {
            T t = default;
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                T right2 = Scalar<T>.Multiply(left.GetElementUnsafe(i), right.GetElementUnsafe(i));
                t = Scalar<T>.Add(t, right2);
            }
            return t;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<int> AsVectorInt32<T>(Vector<T> value)
        {
            return value.As<T, int>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<uint> AsVectorUInt32<T>(Vector<T> value)
        {
            return value.As<T, uint>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool EqualsAll<T>(Vector<T> left, Vector<T> right)
        {
            return left == right;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool GreaterThanAny<T>(Vector<T> left, Vector<T> right)
        {
            for (int i = 0; i < Vector<T>.Count; i++)
            {
                if (Scalar<T>.GreaterThan(left.GetElementUnsafe(i), right.GetElementUnsafe(i)))
                {
                    return true;
                }
            }
            return false;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> Xor<T>(Vector<T> left, Vector<T> right)
        {
            return left ^ right;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static ref T GetElementUnsafe<T>(ref this Vector<T> vector, int index)
        {
            ref T source = ref Unsafe.As<Vector<T>, T>(ref Unsafe.AsRef(ref vector));
            return ref Unsafe.Add(ref source, index);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static void SetElementUnsafe<T>(ref this Vector<T> vector, int index, T value)
        {
            ref T source = ref Unsafe.As<Vector<T>, T>(ref Unsafe.AsRef(ref vector));
            Unsafe.Add(ref source, index) = value;
        }

        public static ref T GetElement<T>(ref this Vector<T> vector, int index)
        {
            if (index >= Vector<T>.Count)
            {
            }
            return ref vector.GetElementUnsafe(index);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static Vector<TTo> As<TFrom, TTo>(this Vector<TFrom> vector)
        {
            return Unsafe.As<Vector<TFrom>, Vector<TTo>>(ref vector);
        }
    }

    [DebuggerTypeProxy(typeof(VectorDebugView<>))]
    public struct Vector<T>
    {
        internal ulong _00;
        internal ulong _01;
        internal ulong _02;
        internal ulong _03;

        public static Vector<T> Zero
        {
            get
            {
                return default;
            }
        }
        public static Vector<T> One
        {
            get
            {
                T one = Scalar<T>.One;
                return new Vector<T>(one);
            }
        }
        public static int Count
        {
            get
            {
                return 8;
            }
        }
        public static bool IsSupported { get => true; }

        public Vector(T value)
        {
            Unsafe.SkipInit(out this);
            for (int i = 0; i < Count; i++)
            {
                this.SetElementUnsafe(i, value);
            }
        }

        public Vector(Span<T> values)
        {
            this = new Vector<T>((ReadOnlySpan<T>)values);
        }

        public Vector(ReadOnlySpan<T> values)
        {
            if (values.Length < Count)
            {
            }
            this = Unsafe.ReadUnaligned<Vector<T>>(ref Unsafe.As<T, byte>(ref MemoryMarshal.GetReference(values)));
        }

        public T this[int index]
        {
            get
            {
                return this.GetElement(index);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> operator *(Vector<T> value, T factor)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Count; i++)
            {
                T value2 = Scalar<T>.Multiply(value.GetElementUnsafe(i), factor);
                result.SetElementUnsafe(i, value2);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> operator *(T factor, Vector<T> value)
        {
            return value * factor;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> operator *(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Count; i++)
            {
                T value = Scalar<T>.Multiply(left.GetElementUnsafe(i), right.GetElementUnsafe(i));
                result.SetElementUnsafe(i, value);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> operator +(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Count; i++)
            {
                T value = Scalar<T>.Add(left.GetElementUnsafe(i), right.GetElementUnsafe(i));
                result.SetElementUnsafe(i, value);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> operator -(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Count; i++)
            {
                T value = Scalar<T>.Subtract(left.GetElementUnsafe(i), right.GetElementUnsafe(i));
                result.SetElementUnsafe(i, value);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> operator -(Vector<T> value)
        {
            return Zero - value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> operator /(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<T> result);
            for (int i = 0; i < Count; i++)
            {
                T value = Scalar<T>.Divide(left.GetElementUnsafe(i), right.GetElementUnsafe(i));
                result.SetElementUnsafe(i, value);
            }
            return result;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> operator &(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<ulong> vector);
            Vector<ulong> vector2 = left.As<T, ulong>();
            Vector<ulong> vector3 = right.As<T, ulong>();
            for (int i = 0; i < Vector<ulong>.Count / 2; i++)
            {
                ulong value = vector2.GetElementUnsafe(i) & vector3.GetElementUnsafe(i);
                vector.SetElementUnsafe(i, value);
            }
            return vector.As<ulong, T>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> operator |(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<ulong> vector);
            Vector<ulong> vector2 = left.As<T, ulong>();
            Vector<ulong> vector3 = right.As<T, ulong>();
            for (int i = 0; i < Vector<ulong>.Count / 2; i++)
            {
                ulong value = vector2.GetElementUnsafe(i) | vector3.GetElementUnsafe(i);
                vector.SetElementUnsafe(i, value);
            }
            return vector.As<ulong, T>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator ==(Vector<T> left, Vector<T> right)
        {
            for (int i = 0; i < Count; i++)
            {
                if (!Scalar<T>.Equals(left.GetElementUnsafe(i), right.GetElementUnsafe(i)))
                {
                    return false;
                }
            }
            return true;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator !=(Vector<T> left, Vector<T> right)
        {
            for (int i = 0; i < Count; i++)
            {
                if (!Scalar<T>.Equals(left.GetElementUnsafe(i), right.GetElementUnsafe(i)))
                {
                    return true;
                }
            }
            return false;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> operator ^(Vector<T> left, Vector<T> right)
        {
            Unsafe.SkipInit(out Vector<ulong> vector);
            Vector<ulong> vector2 = left.As<T, ulong>();
            Vector<ulong> vector3 = right.As<T, ulong>();
            for (int i = 0; i < Vector<ulong>.Count / 2; i++)
            {
                ulong value = vector2.GetElementUnsafe(i) ^ vector3.GetElementUnsafe(i);
                vector.SetElementUnsafe(i, value);
            }
            return vector.As<ulong, T>();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector<T> operator ~(Vector<T> value)
        {
            Unsafe.SkipInit(out Vector<ulong> vector);
            Vector<ulong> vector2 = value.As<T, ulong>();
            for (int i = 0; i < Vector<ulong>.Count / 2; i++)
            {
                ulong value2 = ~vector2.GetElementUnsafe(i);
                vector.SetElementUnsafe(i, value2);
            }
            return vector.As<ulong, T>();
        }

        public override string ToString()
        {
            return ToString("G", CultureInfo.CurrentCulture);
        }

        public unsafe string ToString([StringSyntax("NumericFormat")] string format, IFormatProvider formatProvider)
        {
            var valueStringBuilder = new StringBuilder();
            string numberGroupSeparator = NumberFormatInfo.GetInstance(formatProvider).NumberGroupSeparator;
            valueStringBuilder.Append('<');
            valueStringBuilder.Append(((IFormattable)(object)this.GetElementUnsafe(0)).ToString(format, formatProvider));
            for (int i = 1; i < Count; i++)
            {
                valueStringBuilder.Append(numberGroupSeparator);
                valueStringBuilder.Append(' ');
                valueStringBuilder.Append(((IFormattable)(object)this.GetElementUnsafe(i)).ToString(format, formatProvider));
            }
            valueStringBuilder.Append('>');
            return valueStringBuilder.ToString();
        }
    }
}
